#ifndef _XARM_TASK_HPP
#define _XARM_TASK_HPP

//std include
#include <string>
#include <vector>
#include <stdio.h>
#include <unordered_map>
#include <unordered_set>
#include <chrono>

//ros include
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>

//rm msg include
#include "sensor_msgs/JointState.h"
#include "rm_robot.hpp"

#include "bt_task_msgs/arm_state.h"
#include "bt_task_msgs/set_arm.h"
#include "bt_task_msgs/velo_move_arm.h"
#include "bt_task_msgs/move_arm.h"
#include "bt_task_msgs/clear_error.h"
#include "bt_task_msgs/move_tool_motor.h"


class arm_task_node: public std::enable_shared_from_this<arm_task_node>
{
  public:
    arm_task_node();

    //service callback
    bool set_mode_callback(bt_task_msgs::set_arm::Request& request,
                      bt_task_msgs::set_arm::Response& response);
                      
    bool velo_move_arm_callback(bt_task_msgs::velo_move_arm::Request& request,
                      bt_task_msgs::velo_move_arm::Response& response);

    bool move_arm_callback(bt_task_msgs::move_arm::Request& request,
                      bt_task_msgs::move_arm::Response& response);

    bool clear_error_callback(bt_task_msgs::clear_error::Request& request,
                      bt_task_msgs::clear_error::Response& response);

    bool move_tool_motor_callback(bt_task_msgs::move_tool_motor::Request& request,
                      bt_task_msgs::move_tool_motor::Response& response);

    void init_xarm(const std::string id);
    void init_rm(const std::string id);
    // void reinit_xarm_sub(const std::string id);
    // void shutdown_xarm_client(const std::string id);

  private:
    //ros topic 
    std::unordered_map<std::string, std::shared_ptr<RealmanRobotClient>> _xarm_client_ptr_map;
    std::unordered_map<std::string ,ros::Publisher> xarm_state_pub_map;
    std::unordered_map<std::string, ros::Subscriber> robot_state_sub_map;
    std::unordered_set<std::string> xarm_already_init_list;
    ros::ServiceServer set_mode;
    ros::ServiceServer velo_move_arm_service;
    ros::ServiceServer move_arm_service;
    ros::ServiceServer clear_error_service;
    ros::ServiceServer move_tool_motor_service;

    class subcallback
    {
      public:
      subcallback(ros::NodeHandle nh,std::shared_ptr<arm_task_node> ptr,std::string id)
      :_nh(nh),_ptr(ptr),_id(id){}

      void robot_state_callback(const sensor_msgs::JointState& msg);
      ros::NodeHandle _nh;
      //current id
      std::string _id;
      //the last msg for compare
      bt_task_msgs::arm_state _last_state;
      bool if_get_joint_state = false;
      std::shared_ptr<arm_task_node> _ptr;
    };

    friend class subcallback;

    std::unordered_map<std::string, std::shared_ptr<subcallback>> sub_callback_map;

    //state flags
    struct RobotState
    {
      bool _robot_enabled;
      uint16_t _robot_mode;
      uint16_t _robot_state;
      uint16_t error_code;
      uint16_t warn_code;

      std::vector<float> joint_angle;
      std::vector<float> tcp_position;
    } _robot_state_t;

    std::unordered_map<std::string,int> joint_name_map = {
      {"joint_1",0},{"joint_2",1},{"joint_3",2},
      {"joint_4",3},{"joint_5",4},{"joint_6",5}};
};
#endif